/* Functions for various sensor types */
#include <DFRobot_BME280.h>

#define SEA_LEVEL_PRESSURE  1013.25f
#define BME_CS 10

DFRobot_BME280 bme; //I2C

#define  TEMP_FLAG  1
#define  HUMI_FLAG  2
#define  ALTI_FLAG  3

#define  LEFT_SONAR    1
#define  CENTER_SONAR  2
#define  RIGHT_SONAR   3

#define  A_SONAR_ECHO_PIN  12
#define  A_SONAR_TRIG_PIN  11
#define  B_SONAR_ECHO_PIN   8
#define  B_SONAR_TRIG_PIN   9
#define  C_SONAR_ECHO_PIN   2
#define  C_SONAR_TRIG_PIN   3

#define  DUST_LEDPOWER_PIN  4  //digital pin D4


void initBME280Sensor()
{
  // I2c default address is 0x77, if the need to change please modify bme.begin(Addr)
  if (!bme.begin())
  {
    Serial.println("No sensor device found, check line or address!");
    while (1);
  }
}

void initSonarPinMode()
{
  pinMode(A_SONAR_ECHO_PIN, INPUT);
  pinMode(A_SONAR_TRIG_PIN, OUTPUT);
  digitalWrite(A_SONAR_ECHO_PIN, HIGH);

  pinMode(B_SONAR_ECHO_PIN, INPUT);
  pinMode(B_SONAR_TRIG_PIN, OUTPUT);
  digitalWrite(B_SONAR_ECHO_PIN, HIGH);

  pinMode(C_SONAR_ECHO_PIN, INPUT);
  pinMode(C_SONAR_TRIG_PIN, OUTPUT);
  digitalWrite(C_SONAR_ECHO_PIN, HIGH);
}

void initAllSensor()
{
  pinMode(DUST_LEDPOWER_PIN, OUTPUT);  //init dust led power pinMode
  initBME280Sensor();  //init temp,humi,altitude sensor
  initSonarPinMode();  //init sonar sensor
}

int getDustDensity(int measurePin)
{
  int samplingTime = 280;
  int deltaTime = 40;
  int sleepTime = 9680;

  float voMeasured = 0.0;
  float calcVoltage = 0.0;
  float dustDensity = 0.0; 

  digitalWrite(DUST_LEDPOWER_PIN, LOW); // power on the LED
  delayMicroseconds(samplingTime);

  voMeasured = analogRead(measurePin); // read the dust value

  delayMicroseconds(deltaTime);
  digitalWrite(DUST_LEDPOWER_PIN, HIGH); // turn the LED off
  delayMicroseconds(sleepTime);

  // 0 - 5V mapped to 0 - 1023 integer values recover voltage
  calcVoltage = voMeasured * (5.0 / 1024.0);

  // linear eqaution taken from http://www.howmuchsnow.com/arduino/airquality/
  dustDensity = 0.17 * calcVoltage - 0.1;
  if (dustDensity < 0)
  {
    dustDensity = 0.0;
  }
  
  return dustDensity*1000; // unit: mg/m3 -> ug/m3
}

int getBMESensorData(int flag)
{
  int value = 0;
  switch (flag)
  {
    case TEMP_FLAG:
      value = bme.temperatureValue();
      break;
    case HUMI_FLAG:
      value = bme.humidityValue();
      break;
    case ALTI_FLAG:
      value = bme.altitudeValue(SEA_LEVEL_PRESSURE);
      break;
    default:
      Serial.println("Invalid Command");
      break;
  }
  return value;
}

int getSonarValue(int index)
{
  int echoPin = 0;
  int trigPin = 0;
  int distance = 0;

  if (index == LEFT_SONAR)
  {
    echoPin = A_SONAR_ECHO_PIN;
    trigPin = A_SONAR_TRIG_PIN;
  }
  else if (index == CENTER_SONAR)
  {
    echoPin = B_SONAR_ECHO_PIN;
    trigPin = B_SONAR_TRIG_PIN;
  }
  else
  {
    echoPin = C_SONAR_ECHO_PIN;
    trigPin = C_SONAR_TRIG_PIN;
  }
  digitalWrite(trigPin, LOW); // Set the trigger pin to low for 2uS
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); // Send a 10uS high to trigger ranging
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW); // Send pin low again
  distance = pulseIn(echoPin, HIGH, 26000); // Read in times pulse
  distance = distance / 58;

  return distance;
}

